'''
模块名称：正射校正
模块说明：使用RD模型将像空间坐标系转换为椭球坐标系。
头文件数据格式说明：参考radarsat-2 产品格式说明 https://www.docin.com/p-1747394154.html

V1.0：单例测试没有
V1.1：修改了带宽单位错误导致的bug；增加了批量处理功能； 处理一幅影像花费时间>45天
V1.2：重构了批量处理功能，将前一步的计算结果为后一步的初始值；处理一幅影像花费时间>110小时

'''
import os
from types import resolve_bases
from typing import OrderedDict
import gdal
from numpy.ma.core import power
import xmltodict
from xml.dom.minidom import parse
import pandas
import numpy as np
import json
from matplotlib import pyplot as plt
import datetime
import yaml
import math
from decimal import Decimal # 为了更好的精度
import numba 
from numba import jit

def FindInfomationFromJson(HeaderFile_dom_json,node_path_list):
    '''
    在Json文件中，按照指定路径解析出制定节点
    '''
    result_node=HeaderFile_dom_json
    for nodename in node_path_list:
        result_node=result_node[nodename]
    return result_node

def GetVectorNorm(Vecter):
    '''
    得到向量的模
    '''
    Vecter=Vecter.reshape(-1,1)
    return np.sqrt(np.matmul(Vecter.T,Vecter)) 
    

class SatelliteOrbit(object):
    def __init__(self) -> None:
        super().__init__()
        self.__starttime=1262275200.0

    pass
    def get_starttime(self):
        '''
        返回卫星轨道时间起算点
        '''
        return self.__starttime
    def ReconstructionSatelliteOrbit(self,GPSPoints_list):
        '''
        重建卫星轨道，使用多项式拟合法。
        args:
            GPSPoints_list:GPS卫星轨道点。
        return：
            SatelliteOrbitModel 卫星轨道模型
        '''
        self.SatelliteOrbitModel=None
        pass
    
    pass
    def SatelliteSpaceState(self,time_float):
        '''
        根据时间戳，返回对应时间的卫星的轨迹状态
        args:
            time_float:时间戳
        return：
            State_list:[time,Xp,Yp,Zp,Vx,Vy,Vz]
        '''
        return None
    

class SatelliteOrbit_FitPoly(SatelliteOrbit):
    '''
    继承于SatelliteOribit类，为拟合多项式实现方法
    '''
    def __init__(self) -> None:
        super().__init__()

    def ReconstructionSatelliteOrbit(self,GPSPoints_list,starttime=1262275200.0):
        if len(GPSPoints_list)>16: 
            # 多项式的节点数，理论上是超过4个可以起算，这里为了精度选择16个点起算。
            # 多项式 AX=Y ==> A=（X'X)^X'Y，其中 A 为待求系数，X为变量，Y为因变量
            # 这里使用三次项多项式，共有6组参数。
            # 声明自变量，因变量，系数矩阵
            self.__starttime=starttime
            record_count=len(GPSPoints_list)
            time_arr=np.zeros((record_count,1),dtype=np.float64) # 使用np.float64只是为了精度高些，如果32位也能满足需求，请用32
            state_arr=np.zeros((record_count,6),dtype=np.float64)
            A_arr=np.zeros((4,6),dtype=np.float64)
            # 将点记录转换为自变量矩阵、因变量矩阵
            for i in range(record_count):
                GPSPoint=GPSPoints_list[i]
                time_arr[i,0]=GPSPoint[0]-self.__starttime
                state_arr[i,:]=np.array(GPSPoint[1:]).reshape(1,6)
            # 使用最小二乘法，求解系数
            X=np.concatenate([np.ones((record_count,1),dtype=np.float64),time_arr,time_arr**2,time_arr**3],axis=1)
            
            for i in range(6):
                Y=state_arr[:,i]
                A_arr[:,i]=np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T,X)),X.T),Y).reshape(1,4) # 
            self.A_arr=A_arr
            
            return A_arr
        else:
            self.A_arr=None
            return None
    def SatelliteSpaceState(self,time_float):
        '''
        根据时间戳，返回对应时间的卫星的轨迹状态，会自动计算与起算时间之差
        args:
            time_float:时间戳
        return：
            State_list:[time,Xp,Yp,Zp,Vx,Vy,Vz]
        '''
        if self.A_arr is None:
            return None
        time_float=time_float-self.__starttime
        tiem_arr=np.array([1,time_float,time_float**2,time_float**3]).reshape(1,4)
        SatelliteState_xyz_Vxyz_arr=np.matmul(tiem_arr,self.A_arr).reshape(1,6)
        return np.concatenate([np.array(time_float+self.__starttime).reshape(1,1),SatelliteState_xyz_Vxyz_arr],axis=1)

def ReconstructionSatelliteOrbit(GPSPoints_list,starttime=1262275200.0):
    '''
    构建卫星轨道
    args:
        GPSPoints_list:卫星轨道点
        starttime:起算时间，默认设置为2010年1月1日，0时0分0秒 
    '''
    if len(GPSPoints_list)>16:
        SatelliteOrbitModel=SatelliteOrbit_FitPoly()
        if SatelliteOrbitModel.ReconstructionSatelliteOrbit(GPSPoints_list,starttime=starttime) is None:
            return None
        return SatelliteOrbitModel


class Orthorectification(object):
    '''
    正射校正模块类。
    '''

    def __init__(self,configPath="./config.ymal") -> None:
        super().__init__()
        # 常量声明
        # 加载配置文件
        with open(configPath,'r',encoding='utf-8') as f:
            cont = f.read()
            self.config = yaml.load(cont)
        self.config['SatelliteOribit']['StartTime']=datetime.datetime.strptime(self.config['SatelliteOribit']['StartTime']['Value'],self.config['SatelliteOribit']['StartTime']['format']).timestamp()
        self.R_e=self.config['SatelliteOribit']['ReferenceSpheroid']['Re'] # m
        self.R_p=self.config['SatelliteOribit']['ReferenceSpheroid']['Rp'] # m
        self.w_e=self.config['SatelliteOribit']['ReferenceSpheroid']['we'] # rad/s        

    def ParseHearderFile(self,HeaderFilePath_str,GPSNodeNames_list=['TimeStamp', 'xPosition', 'yPosition', 'zPosition', 'xVelocity', 'yVelocity', 'zVelocity']):
        '''
        解析头文件，返回计算所需要的必要信息:
        args:
            HeaderFilePath_str: 头文件地址，
        return:
            HeaderFileInfomation_json:头文件信息包
        '''

        with open(HeaderFilePath_str,'r',encoding='utf-8') as fp:
            HeaderFile_dom_str=fp.read()
        HeaderFile_dom=xmltodict.parse(HeaderFile_dom_str)  
        HeaderFile_dom_json=json.loads(json.dumps(HeaderFile_dom))
        # 获取头文件
        HeaderInformation_json={}
        # 解析轨道节点，假定是GPS节点。
        HeaderInformation_json['GPS']=[] 
        # GPS 解析信息
        GPSNode_Path=self.config["GPS"]['GPSNode_Path']
        GPSNodeNames_list=self.config["GPS"]['NodeInfomation_Name']
        GPSTime_Format=self.config['GPS']['Time_format']
        GPSPoints=FindInfomationFromJson(HeaderFile_dom_json,GPSNode_Path)
        for GPSPoint in GPSPoints:
            GPSPoint=[
                    datetime.datetime.strptime(GPSPoint[GPSNodeNames_list[0]],GPSTime_Format).timestamp(), # TimeStamp
                    float(GPSPoint[GPSNodeNames_list[1]]), # Xp
                    float(GPSPoint[GPSNodeNames_list[2]]), # Yp
                    float(GPSPoint[GPSNodeNames_list[3]]), # Zp
                    float(GPSPoint[GPSNodeNames_list[4]]), # Vx
                    float(GPSPoint[GPSNodeNames_list[5]]), # Vy
                    float(GPSPoint[GPSNodeNames_list[6]])] # VZ
            HeaderInformation_json["GPS"].append(GPSPoint)
        # 提取成像时间信息
        HeaderInformation_json['ImageInformation']={}
        # 1、开始成像时间
        HeaderInformation_json['ImageInformation']['StartTime']=datetime.datetime.strptime(
                FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['StartImageTime']['NodePath']),
                self.config['imageinfo']['StartImageTime']['Format']
        ).timestamp()
        # 2、影像结束成像时间
        HeaderInformation_json['ImageInformation']['EndTime']=datetime.datetime.strptime(
                FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['EndImageTime']['NodePath']),
                self.config['imageinfo']['StartImageTime']['Format']
        ).timestamp()
        # 3、影像的宽高
        HeaderInformation_json['ImageInformation']['height']=int(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['ImageHeight']['NodePath']))
        HeaderInformation_json['ImageInformation']['width']=int(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['ImageWidth']['NodePath']))
        # 4、影像的近斜距
        HeaderInformation_json['ImageInformation']['NearRange']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['NearRange']['NodePath']))
        # 5、入射角
        HeaderInformation_json['ImageInformation']['incidenceAngle']={
            'NearRange':float(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['incidenceAngle']['NearRange']['NodePath'])),
            'FarRange':float(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['incidenceAngle']['FarRange']['NodePath']))
        }
        # 6、成像时刻影像带宽
        HeaderInformation_json['ImageInformation']['bandWidth']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['sensor']['bandWidth']['NodePath']))
        # 7、多普勒质心常数
        DopplerCentroidCoefficients_list=FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['DopplerCentroidCoefficients']['NodePath'])
        HeaderInformation_json['ImageInformation']['DopplerCentroidCoefficients']=[
            float(DopplerCentroidCoefficients_list[self.config['imageinfo']['DopplerCentroidCoefficients']['DopplerCentroidCoefficients_Name'][0]]),
            float(DopplerCentroidCoefficients_list[self.config['imageinfo']['DopplerCentroidCoefficients']['DopplerCentroidCoefficients_Name'][1]]),
            float(DopplerCentroidCoefficients_list[self.config['imageinfo']['DopplerCentroidCoefficients']['DopplerCentroidCoefficients_Name'][2]]),
            float(DopplerCentroidCoefficients_list[self.config['imageinfo']['DopplerCentroidCoefficients']['DopplerCentroidCoefficients_Name'][3]]),
            float(DopplerCentroidCoefficients_list[self.config['imageinfo']['DopplerCentroidCoefficients']['DopplerCentroidCoefficients_Name'][4]]),
        ]
        # 8、波长
        HeaderInformation_json['ImageInformation']['lambda']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['sensor']['lambda']['NodePath']))
        # 9、中心像元对应得中心经纬度
        CenterPoint=FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['CenterImagePositon']['NodePath'])
        HeaderInformation_json['ImageInformation']['centerPosition']=[
            float(CenterPoint[self.config['imageinfo']['CenterImagePositon']['Value'][0]]), # 纬度
            float(CenterPoint[self.config['imageinfo']['CenterImagePositon']['Value'][1]]), # 经度
        ]
        # 10、多普勒参数参考时间
        HeaderInformation_json['ImageInformation']['DopplerParametersReferenceTime']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['DopplerParametersReferenceTime']['NodePath']))
        # 11、参考斜距
        HeaderInformation_json['ImageInformation']['refRange']=float(FindInfomationFromJson(HeaderFile_dom_json,self.config['imageinfo']['ReferenceRange']['NodePath']))
        return HeaderInformation_json
        pass
   
    def LLA_to_XYZ(self,latitude, longitude, altitude):
        ''' 经纬度转大地坐标系
        args:
            latitude:纬度
            longitude:经纬
            altitude:海拔高程
        refrence: https://blog.csdn.net/dulingwen/article/details/96868530
        '''
        # 经纬度的余弦值
        cosLat = math.cos(latitude * math.pi / 180)
        sinLat = math.sin(latitude * math.pi / 180)
        cosLon = math.cos(longitude * math.pi / 180)
        sinLon = math.sin(longitude * math.pi / 180)
    
        # WGS84坐标系的参数
        rad = 6378137.0        #地球赤道平均半径（椭球长半轴：a）
        f = 1.0 / 298.257224   # WGS84椭球扁率 :f = (a-b)/a
        C = 1.0 / math.sqrt(cosLat * cosLat + (1-f) * (1-f) * sinLat * sinLat)
        S = (1-f) * (1-f) * C
        h = altitude
    
        # 计算XYZ坐标
        X = (rad * C + h) * cosLat * cosLon
        Y = (rad * C + h) * cosLat * sinLon
        Z = (rad * S + h) * sinLat
    
        return np.array([X, Y, Z]).reshape(1,3)
   
    def XYZ_to_LLA(self,X, Y, Z):
        ''' 大地坐标系转经纬度
        适用于WGS84坐标系
        args:
            x,y,z
        return:
            lat,long,altitude
        '''
        # WGS84坐标系的参数
        a = 6378137.0        # 椭球长半轴
        b = 6356752.314245   # 椭球短半轴
        ea = np.sqrt((a ** 2 - b ** 2) / a ** 2)
        eb = np.sqrt((a ** 2 - b ** 2) / b ** 2)
        p = np.sqrt(X ** 2 + Y ** 2)
        theta = np.arctan2(Z * a, p * b)
    
        # 计算经纬度及海拔
        longitude = np.arctan2(Y, X)
        latitude = np.arctan2(Z + eb ** 2 * b * np.sin(theta) ** 3, p - ea ** 2 * a * np.cos(theta) ** 3)
        N = a / np.sqrt(1 - ea ** 2 * np.sin(latitude) ** 2)
        altitude = p / np.cos(latitude) - N
    
        return np.array([np.degrees(latitude), np.degrees(longitude), altitude])

 
    def getRByColumncode(self,c):
        '''
        根据列号计算斜距
        args:
            c：列号
        '''
        return  self.R0+c*self.delta_R
        pass
 
    def getTimeByLineCode(self,r):
        '''
        根据行号计算成像时间
        args:
            r：行号
        '''
        return np.matmul(np.concatenate([ np.array([r**i]).reshape(1,1) for i in range(self.r2t_A_arr.shape[0])],axis=1),self.r2t_A_arr)


    def ReconstuctionTimesOflyDirectionPositionModel(self,timePoints_list):
        ''' 构建飞行向坐标与时间的转换模型,注意这里没有调整时间的起算点。
            args:
                timePoints_list:时间点坐标 [r,t]
        '''
        # 根据点数确定模型，最高为3次项模型
        Y=np.zeros((len(timePoints_list),1)) # 确定Y
        X=np.zeros((len(timePoints_list),1))
        for i in range(len(timePoints_list)):
            Y[i,0]=timePoints_list[i][1]
            X[i,0]=timePoints_list[i][0]
        if len(timePoints_list)==2:
            # 一次项
            X=np.concatenate([np.ones((len(timePoints_list),1)),X],axis=1)
            A=np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T,X)),X.T),Y)

        elif len(timePoints_list)>=3: # 二次项
            X=np.concatenate([np.ones(len(timePoints_list),1),X,np.power(X,2)],axis=1)
            A=np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T,X)),X.T),Y)
            pass
        elif len(timePoints_list)>=8: # 三次项
            X=np.concatenate([np.ones(len(timePoints_list),1),X,np.power(X,2),np.power(X,3)],axis=1)
            A=np.matmul(np.matmul(np.linalg.inv(np.matmul(X.T,X)),X.T),Y)
            pass
        self.r2t_A_arr=A

    def PrepareConverteSystem(self):
        ''' 计算常量
        在数据计算之前，提前处理部分常量信息
        1、构建r,c 到坐标斜距的计算公式
        '''
        R0=self.header_info['ImageInformation']['NearRange'] # 起始斜距，近斜距
        delta_R=self.config['LightSpeed']/(2*self.header_info['ImageInformation']['bandWidth']*10**6)# 计算距离向分辨率 ,参考《InSAR原理与应用》P36，距离向分辨率 带宽单位为MHZ
        self.ReconstuctionTimesOflyDirectionPositionModel([
            [0,self.header_info['ImageInformation']['StartTime']],
            [self.header_info['ImageInformation']['height']-1,self.header_info['ImageInformation']['EndTime']]
            ]) # 构建坐标到时间的变换
        self.R0=R0
        self.delta_R=delta_R

    
    
    def Orthorectification(self,FilePath_str):
        '''
        正射校正组件。
        正射校正整体可以分为两个步骤：
        Step 1：计算出栅格坐标对应的真实的大地坐标系坐标
        Step 2：根据大地坐标系将影像重采样到大地坐标系空间中。
        args:
            FilePath_str:影像所在文件夹
        '''
        # 分离需要校正对象
        file_name_list=os.listdir(FilePath_str) 
        header_name=[file_name for file_name in file_name_list if file_name.rfind(".meta.xml")==len(file_name)-len('.meta.xml')][0] # 头文件
        tiff_name=[file_name for file_name in file_name_list if file_name.rfind(".tiff")==len(file_name)-len('.tiff')] # 影像文件
        # 解析头文件
        self.header_info=self.ParseHearderFile(os.path.join(FilePath_str,header_name))
        # 构建轨道模型
        self.SatelliteOrbitModel=ReconstructionSatelliteOrbit(self.header_info['GPS'],starttime=self.config['SatelliteOribit']['StartTime']) # 构建卫星轨道模型,取第0个节点的时间
        # 求解模型前计算常数变换模型
        self.PrepareConverteSystem()
        # 测试第0，0个点的空间坐标
        self.ConverteCoordinaryPoint_single(0,0)
        # 批量计算空间坐标
        self.ConverteCoordinary()

   
    def ConverteCoordinary(self):
        '''
        批量求解点坐标
        '''
        pass
   
    def ConverteCoordinaryPoint_single(self,r,c,h=0,breakCondition=1):
        '''
        求解单个点坐标
        args:
            r: 行号 height
            c: 列号 width
            h：平均大地高
            breakCondition: 迭代终止条件，默认值为1
        '''
        pass



        

class Orthorectification_RD(Orthorectification):
    '''
    正射校正模块类。
    '''
    def __init__(Orthorectification_RD,configPath="./config.ymal") -> None:
        super().__init__(configPath)  

    def ConverteCoordinary(self):
        '''
        批量求解点坐标
        '''
        # 构建映射表 ----------------------------------需要重写代码
        result={}
        print("开始时间：",datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f"))
        alpha0=0
        beta0=0
        for r in range(self.header_info['ImageInformation']['height']):
            if r%10==0: 
                print("r={} 时间：".format(str(r)),datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f"))
            for c in range(self.header_info['ImageInformation']['width']):
                result[(r,c)]=self.ConverteCoordinaryPoint_single(r,c,alpha0=alpha0,beta0=beta0)
                alpha0=result[(r,c)][1]
                beta0=result[(r,c)][2]
        print("终止时间：",datetime.datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f"))
        pass
    

    def ConverteCoordinaryPoint_single(self,r,c,h=0,breakCondition=1,alpha0=0,beta0=0):
        '''
        求解单个点坐标
        args:
            r: 行号 height
            c: 列号 width
            h：平均大地高
            breakCondition: 迭代终止条件，默认值为1
        return:
            TargetPosition_:点坐标
        版本改动： 
        2021.8.19  尝试将椭球方程进行变形 
        2021.8.22  方法重写
        '''
        # 计算对应成像时间和成像斜距
        R=self.getRByColumncode( c) # 斜距
        t=self.getTimeByLineCode( r) # 成像时间
        # 解析成像时刻，卫星空间位置
        SatelliteState=self.SatelliteOrbitModel.SatelliteSpaceState(t)  
        [Sate_time,Sate_x,Sate_y,Sate_z,Sate_vx,Sate_vy,Sate_vz]=SatelliteState.reshape(1,7).tolist()[0]  
        # 多普勒
        Dopper_d=np.array(self.header_info['ImageInformation']['DopplerCentroidCoefficients']).reshape(-1,1)
        LightSpeed=self.config['LightSpeed']
        # DopplerParametersReferenceTime=self.header_info['ImageInformation']['DopplerParametersReferenceTime']

        DopplerParametersReferenceTime=self.header_info['ImageInformation']['refRange']*2/LightSpeed
        # 波长
        lamda=self.header_info['ImageInformation']['lambda']
        # Re,Rp,we
        Re=self.config['SatelliteOribit']['ReferenceSpheroid']['Re']
        Rp=self.config['SatelliteOribit']['ReferenceSpheroid']['Rp']
        we=self.config['SatelliteOribit']['ReferenceSpheroid']['we'] # we 地球自转速度矢量 
        # 使用牛顿迭代法，求解Xp,Yp,Zp 
        # 椭球体方程常数 a+h ==>a  b==>b
        a=(Re+h)**2
        b=Rp**2
        # 初始迭代点为中心像元经纬度对应的大地坐标系
        #XYZ_=self.LLA_to_XYZ(self.header_info['ImageInformation']['centerPosition'][0],
                            #self.header_info['ImageInformation']['centerPosition'][1],
                            #h)
        XYZ=np.array([Sate_x,Sate_y,Sate_z]).reshape(1,3)
        # 确定初始值，作为求解起始值
        TargetPosition=math.sqrt( 1/((Sate_x**2+Sate_y**2)/a+(Sate_z**2)/b))
        TargetPosition=TargetPosition*np.array([Sate_x,Sate_y,Sate_z]).reshape(1,3)
        TargetPosition=TargetPosition.reshape(1,3)
        R_threshold= np.sqrt( np.sum( np.power(TargetPosition-XYZ,2) ))
        SatellitePosition=np.array([Sate_x,Sate_y,Sate_z]).reshape(3,1)
        SatelliteVelocity=np.array([Sate_vx,Sate_vy,Sate_vz]).reshape(3,1)
        TargetPosition_,alpha,beta=self.ASF(R,SatellitePosition,SatelliteVelocity,TargetPosition,Dopper_d,LightSpeed,DopplerParametersReferenceTime,lamda,R_threshold,H=h,alpha0=alpha0,beta0=beta0)
        ''' 
        # 中间检查有无错误
        TargetPositionLLA=self.XYZ_to_LLA(TargetPosition[0,0],TargetPosition[0,1],TargetPosition[0,2]).reshape(1,-1)
        TargetPosition_LLA=self.XYZ_to_LLA(TargetPosition_[0,0],TargetPosition_[0,1],TargetPosition_[0,2]).reshape(1,-1)
        np.set_printoptions(suppress=True)
        print(TargetPosition.reshape(1,-1))
        print(TargetPosition_.reshape(1,-1))
        print(TargetPositionLLA.reshape(1,-1))
        print(TargetPosition_LLA.reshape(1,-1))
        print(TargetPosition_-TargetPosition)
        print(TargetPosition_LLA-TargetPositionLLA)
        np.set_printoptions(suppress=False)
        '''
        return TargetPosition_.reshape(1,-1),alpha,beta


    
    '''
    RD模型解算模块,这里主要使用ASF方法
    reference: 陈尔学. 星载合成孔径雷达影像正射校正方法研究[D].中国林业科学研究院,2004. 第三章内容
                且假定地表运动速度为0
    '''
   
    def ASF(self,R,SatellitePosition,SatelliteVelocity,TargetPosition,Dopper_d,LightSpeed,DopplerParametersReferenceTime,lamda,R_threshold,H=0,alpha0=0,beta0=0):
        # 部分参数初始化
        alpha=alpha0
        beta=beta0
        delta_alpha=0


        Dopper_d=Dopper_d.reshape(-1,1)
        # 这里需要重构
        TSR=2*R/LightSpeed
        T0=DopplerParametersReferenceTime
        TSR=TSR-T0
        # 这个公式是根据《RADARSAT-2 PRODUCT FORMAT DEFINITION》(RadarSat2的数据格式手册) 
        # “table 5-17 Doppler Rate Values” 中 dopplerRateValuesCoefficients 一行
        # ist of up to 5 Doppler rate values coefficients as a function of slant range time: r0, r1, r2, r3, and r4 where: 
        # Doppler rate  values = r0 + r1(tSR - t0) + r2(tSR-t0)^2 + r3(tSR-t0)^3 + r4(tSR-t0)^4
        fd=np.matmul(np.array([TSR**0,TSR**1,TSR**2,TSR**3,TSR**4]).reshape(1,5),Dopper_d)   
        we=self.w_e
        M=self.SatelliteCoordination2ECR(SatellitePosition,SatelliteVelocity)
        i=0 # 统计迭代次数
        while True:
            TargetVelocity=np.array([0,0,0]).reshape(3,1)# np.array([-1*we*TargetPosition[1,0],we*TargetPosition[0,0],0]) # 计算地面速度,粗糙计算时，默认为0
            beta=self.GetLookFromRangeYaw(R,alpha,beta,SatellitePosition,SatelliteVelocity,R_threshold,H=H)
            FD_=self.FD(alpha,beta,SatelliteVelocity,TargetVelocity,R,lamda,M)
            delta_fd=FD_-fd
            delta_alpha=-1*delta_fd*lamda/(2* np.sum(np.sqrt(np.power(SatelliteVelocity-TargetVelocity,2))))
            if np.abs(delta_alpha*R)<0.1:
                XYZ=self.GetXYZByBetaAlpha(alpha,beta,SatellitePosition,R,M)
                break
            alpha=alpha+delta_alpha
            i=i+1
        return XYZ.reshape(1,-1),alpha,beta

   
    def GetLookFromRangeYaw(self,R,alpha,beta,SatellitePosition,SatelliteVelocity,R_threshold,H=0):
        '''
        根据R和alpha 计算出beta来。
        根据参考论文，式3-13，完成此方法。（注意这是近似法
        args:
            R：斜距
            alpha:雷达侧视角
            beta0:beta初始值
            SatellitePosition:3x1 卫星空间位置
            TargetPosition：3x1 目标对象
        '''
        M=self.SatelliteCoordination2ECR(SatellitePosition,SatelliteVelocity)
        # 地球半径
        Rp=self.R_p+H
        Rs_norm=math.sqrt(np.matmul(SatellitePosition.T,SatellitePosition).reshape(1,1)[0,0])
        # 初始化beta
        if beta==0:
            beta_cos=(Rs_norm**2+R**2-Rp**2)/(2*Rs_norm*R)
            beta=math.acos(beta_cos)
        # 迭代
        i=0
        while True:
            # 计算斜率的增加
            delta_R=R-self.FR(alpha,beta,SatellitePosition,M,R_threshold,R,H=H) 
            # 计算入射角
            sin_eta=Rs_norm*math.sin(beta)/Rp
            tan_eta=sin_eta/math.sqrt(1-sin_eta**2)
            # 计算增量
            delta_beta=delta_R/(R*tan_eta)
            # 更新
            beta=beta+delta_beta
            # 判断循环终止条件
            if np.abs(delta_R)<0.1:
                break
            i=i+1
            if i>=10000: # 达到终止条件
                return None
        return beta
   
    def XYZOuter(self,A,B):
        '''
        外积（叉乘）,日后版本换成可以任意维度的外积运算方程
        args:
          A:3x1
          B:3x1
        '''
        C=np.cross(A.reshape(1,3),B.reshape(1,3)).reshape(-1,1)
        return C

    def SatelliteCoordination2ECR(self,SatellitePosition,SatelliteVelocity):
        '''
        将卫星为中心的直角坐标系Sxyz转换为ECR坐标系。
        M=[x,y,z]
        args:
            SatellitePosition:np.array 3x1  ECR坐标系下，卫星空间位置
            SatelliteVelocity:np.array 3x1   ECR坐标系下，卫星运动速度
        return:
            坐标系变换矩阵 M
            ECR=M*Sxyz
            P‘=MP
        '''
        z=SatellitePosition/GetVectorNorm(SatellitePosition)

        y=self.XYZOuter(SatellitePosition,SatelliteVelocity)/(np.sqrt(np.matmul(SatellitePosition.T,SatellitePosition))*np.sqrt(np.matmul(SatelliteVelocity.T,SatelliteVelocity))) # 外积
        x=self.XYZOuter(y,z)
        M=np.concatenate([x.reshape(3,1),y.reshape(3,1),z.reshape(3,1)],axis=1) # 以列组合
        return M
   
    def UnitVectorOfSatelliteAndTarget(self,alpha,beta,M):
        '''
        获取从卫星指向地面目标的单位矢量
        '''
        # 目标T的单位向量 ST=R*M*P
        P=np.array([math.sin(alpha),-math.cos(alpha)*math.sin(beta),-math.cos(alpha)*math.cos(beta),]).reshape(3,1)
        P_=np.matmul(M,P).reshape(-1,1)
        return P_
   
    def FD(self,alpha,beta,SatelliteVelocity,TargetVelocity,R,lamda,M):
        '''
        根据beta,alpha,卫星空间位置,斜距，转换矩阵
        '''
        we=self.w_e
        #
        TargetVelocity=0 # 假设地面运动为0
        UnitVector=self.UnitVectorOfSatelliteAndTarget(alpha,beta,M)
        Rst=-1*R*UnitVector
        test_velocity=GetVectorNorm(SatelliteVelocity)
        Rst_Vst=np.matmul(Rst.T,SatelliteVelocity-TargetVelocity)/R
        fd=-1*(2/lamda)*Rst_Vst
        return fd
   
    def GetXYZByBetaAlpha(self,alpha,beta,SatellitePosition,R,M):
        '''
        从beta，alpha获取获取目标的空间位置
        '''
        P=np.array([math.sin(alpha),-math.cos(alpha)*math.sin(beta),-math.cos(alpha)*math.cos(beta),]).reshape(3,1)
        Rt=SatellitePosition+R*np.matmul(M,P)
        return Rt
   
    def FR(self,alpha,beta,SatellitePosition,M,R_threshold,R_ref,H=0):
        '''
        根据 雷达侧视角，雷达视角，卫星位置，坐标系变换矩阵，大地高等参数，根据参考论文中公式3-1 ==》3-10的计算过程，设计此方程
        args:
            alpha:雷达侧视角 卫星与地物间速度运动差导致的（即多普勒频移的结果）
            beta: 雷达视角
            Satellite_position:卫星空间位置
            M：坐标系转换矩阵
            H：大地高
        return:
            R：斜距
        '''
       
        UnitVector=self.UnitVectorOfSatelliteAndTarget(alpha,beta,M)
        # 解方程 3-10 纠正错误，A公式有问题
        a=(self.R_e+H)**2
        b=self.R_p**2
        [Xs,Ys,Zs]=SatellitePosition.reshape(-1,1)
        [Xp,Yp,Zp]=UnitVector.reshape(-1,1)
        A=(Xp**2+Yp**2)/a+(Zp**2)/b
        B=2*(Xs*Xp+Ys*Yp)/a+2*Zs*Zp/b
        C=(Xs**2+Ys**2)/a+Zs**2/b
        C=C-1
        t=B**2-4*A*C
        #R=(-B+math.sqrt(t))/(2*A) # 这里通过几何分析排除这个解
        R1=(-B-math.sqrt(t))/(2*A)
        if R1>R_threshold:
            return R1
        return None

# 数据测试模块
if __name__=="__main__":
    tiff_path="./data/4-GF3_KAS_FSII_020008_E113.2_N23.1_20200528_L1A_HHHV_L10004829485/4-GF3_KAS_FSII_020008_E113.2_N23.1_20200528_L1A_HHHV_L10004829485"
    Orthorectification_RD_object=Orthorectification_RD()
    Orthorectification_RD_object.Orthorectification(tiff_path)
    pass
